#include "../liolib/lio3d.h"
#include "../liolib/lionav.h"
#include "../liolib/liocom.h"
//#include "Random.h"

namespace Lio {

NumberFormat nf;

double e=E_EARTH,ae=A_EARTH;
//Rm=ae*(1-e*e)*df1w(lat);
double df1w(double lat, double x)
{
	return pow(1-pow2(e*sin(lat)),-1.5);
}
double df1w(double lat)
{
	return pow(1-pow2(e*sin(lat)),-1.5);
}
//Rt=ae*df2w(lat)
double df2w(double lat)
{
	return cos(lat)*pow(1-pow2(e*sin(lat)),-0.5);
}
inline double func(double x)
{
	return sin(x);
}
// Get n Interpolation Coefs For y=f(x-x0) By Matrix Invertion
BOOL getInterCoefMI(double *coef,
					int n,
					const double *y, // n Values For y
					const double *x, // n Values For x
					double x0)
{
	if(!(y&&x&&n>1&&coef))
		return FALSE;
	int i,j;
	double xij;
	Matrix X,Y,C,Xinv;
	if(!X.setSize(n,n)||!Xinv.setSize(n,n)||!Y.setSize(n,1)||!C.setSize(n,1))
		return FALSE;
	for(i=0;i<n;i++)
	{
		xij=1;
		for(j=0;j<n;j++)
		{
			X.elems[i][j]=xij;
			xij*=(x[i]-x0);
		}
		Y.elems[i][0]=y[i];
	}
	if(!invMatrix(X.elems,Xinv.elems,n))
		return FALSE;
	mulMatrix(Xinv.elems,Y.elems,C.elems,n,n,1);
	for(i=0;i<n;i++)
		coef[i]=C.elems[i][0];
	return TRUE;
}
void testInter()
{
	int nPoints=5,nValues=10;
	double x,x0=0,xStart=-PI/2,xEnd=PI/2,xStep,y_r,y_i,y_i_mi;
	xStep=(xEnd-xStart)/(nValues-1);
	Vector<double> yValues,xValues,coefs_in,coefs_mi,errors,errors_mi;
	Stat interStat,interMIStat;

	yValues.setSize(nPoints,TRUE);
	xValues.setSize(nPoints,TRUE);
	coefs_in.setSize(nPoints,TRUE);
	coefs_mi.setSize(nPoints,TRUE);
	errors.setSize(nValues,TRUE);
	errors_mi.setSize(nValues,TRUE);

	incData(xStart,xEnd,nPoints,xValues.elems);
	PRINT(_T("x: "));
	PRINT_VECTOR(_T("%e, "),xValues.elems,nPoints);
	PRINT(_T("\n"));

	incData(xStart,xEnd,nPoints,yValues.elems,func);
	PRINT(_T("y: "));
	PRINT_VECTOR(_T("%e, "),yValues.elems,nPoints);
	PRINT(_T("\n"));

	if(!getInterCoef(coefs_in.elems,nPoints,yValues.elems,xValues.elems,x0))
	{
		PRINT(_T("coefs_in failed!"));
		return;
	}
	PRINT(_T("coefs_in: "));
	PRINT_VECTOR(_T("%e, "),coefs_in.elems,nPoints);
	PRINT(_T("\n"));

	if(!getInterCoefMI(coefs_mi.elems,nPoints,yValues.elems,xValues.elems,x0))
	{
		PRINT(_T("coefs_mi failed!"));
		return;
	}
	PRINT(_T("coefs_mi: "));
	PRINT_VECTOR(_T("%e, "),coefs_mi.elems,nPoints);
	PRINT(_T("\n"));

	for(int i=0;i<nValues;i++)
	{
		x=xStart+i*xStep;
		PRINT(_T("\nx = %f\n"),x*RAD_TO_DEG);
		y_r=func(x);
		y_i=funcValue(x,coefs_in.elems,nPoints,x0);
		y_i_mi=funcValue(x,coefs_mi.elems,nPoints,x0);
		PRINT(_T("y real     = %+.12e\n"),y_r);
		PRINT(_T("y inter in = %+.12e\n"),y_i);
		PRINT(_T("y inter mi = %+.12e\n"),y_i_mi);
		errors.elems[i]=y_i-y_r;
		errors_mi.elems[i]=y_i_mi-y_r;
		interStat.add(errors.elems[i]);
		interMIStat.add(errors_mi.elems[i]);
	}
	interStat.update();
	interMIStat.update();
	PRINT(_T("\nSTAT:\n"));
	PRINT(_T("inter mean in = %+.12e\n"),interStat.mean);
	PRINT(_T("inter mean mi = %+.12e\n"),interMIStat.mean);
	PRINT(_T("inter dev in  = %.12e\n"),interStat.stdev);
	PRINT(_T("inter dev mi  = %.12e\n"),interMIStat.stdev);
}
void testIntegError()
{
	const int N=1000000,M=3;
	const double dt=1e-3,A_MAX=100;
	register int n,m;
	double	A[M],w[M],tmp1[M],tmp2[M],
			dx,dxe,x=0,xe=0,t,t0=1,tmp,wmax=0,
			mean=0,var=0,var1=0,var2=0,var3=0;

	for(m=0;m<M;m++)
	{
		w[m]=random(0,0.5*PI/dt);
		A[m]=random(0,A_MAX);
		tmp=w[m]*dt;
		tmp1[m]=2*sin(0.5*tmp)/tmp;
		tmp2[m]=sin(tmp)/tmp;
		if(wmax<w[m])
			wmax=w[m];
		var1+=(tmp1[m]*tmp1[m]-2*tmp2[m]+1)*A[m]*A[m];
		var2+=(pow2(1-tmp*tmp/24)-2*(1-tmp*tmp/6)+1)*A[m]*A[m];
		var3+=pow2(0.5*A[m]*tmp);
	}
	tmp=0.5*dt*dt;
	var1*=tmp;
	var2*=tmp;
	var3*=tmp;
	_tprintf(_T("T =%.3e, dt=%.3e, dt_max=%.3e\n"),N*dt,dt,PI/(2*wmax));
	_tprintf(_T("var_est3=% e\n"),var3);
	_tprintf(_T("var_est2=% e\n"),var2);
	_tprintf(_T("var_est1=% e\n"),var1);
	for(n=0,t=t0;n<N;n++,t+=dt)
	{
		dxe=A[0]*sin(w[0]*t)*dt;
		dx=A[0]*sin(w[0]*(t+0.5*dt))*tmp1[0]*dt;
		for(m=1;m<M;m++)
		{
			dxe+=A[m]*sin(w[m]*t)*dt;
			dx+=A[m]*sin(w[m]*(t+0.5*dt))*tmp1[m]*dt;
		}
		xe+=dxe;
		x+=dx;
		mean+=dx-dxe;
		var+=(dx-dxe)*(dx-dxe);
	}
	mean/=N;
	var=var/N-mean*mean;
	_tprintf(_T("var_real=% e\n"),var);
	_tprintf(_T("mean    =% e\n"),mean);
	_tprintf(_T("\nx =% e\n"),x);
	_tprintf(_T("xe=% e\n"),xe);
	_tprintf(_T("e= % e\n"),x-xe);
}

void testIntegPosition()
{
	const int N=100000000;
	const double v0=1,a0=-9.8,r0=1,t0=0,dt=0.01;
	register int i;
	double dr=0,dre=0,dv=0,dve=0,r,re,v,ve,a,ae,t,mean,var;

	r=re=r0;
	v=ve=v0;
	a=ae=a0;
	t=t0;
	mean=var=0;
	for(i=0;i<N;i++)
	{
		dr=INV(i+1);
		mean+=dr;
		var+=dr*dr;
	}
	_tprintf(_T("S1=%e\n"),mean);
	_tprintf(_T("S2=%e\n"),var);
	/*mean/=N;
	var=var/N-mean*mean;
	_tprintf(_T("T =%e, dt=%e, w=%e, A=%e\n"),N*dt,dt,w,A);
	_tprintf(_T("\nx =% e\n"),x);
	_tprintf(_T("xe=% e\n"),xe);
	_tprintf(_T("e= % e\n"),x-xe);
	_tprintf(_T("\nm= % e\n"),mean);
	_tprintf(_T("vr =%e\n"),var);
	_tprintf(_T("ve1=%e\n"),(tmp1*tmp1-2*tmp2+1)*0.5*pow2(A*dt));
	tmp1=1-pow2(w*dt)/24;
	tmp2=1-pow2(w*dt)/6;
	_tprintf(_T("ve2=%e\n"),(tmp1*tmp1-2*tmp2+1)*0.5*pow2(A*dt));
	_tprintf(_T("ve3=%e\n"),0.2*pow2(A*w*dt*dt));*/
}
void testPoly()
{
	int i=0,j=0,k=0;
	FILE *f=NULL;
	int nSteps=100000,nCoef;
	const int nLats=91,nCoefN=20,nCoef0=10;
	double diError[nLats],iError[nLats];
	double lat=toRad(0),lat0=0,x0=0,factor=ae*(1-e*e);
	double x=factor*PI/2;
	double em[nCoefN-nCoef0+1],ed[nCoefN-nCoef0+1];
	double minError=0;
	int minCoef=-1;
	Vector<double> latValues,dxValues,diCoefs;
	_tprintf(	_T("Start Position: %f, Integration Steps: %d\n"),x0,nSteps);
	for(nCoef=nCoef0;nCoef<=nCoefN;nCoef++)
	{
		latValues.setSize(nCoef);
		dxValues.setSize(nCoef);
		incData(-PI/2,PI/2,nCoef,latValues.elems);
		incData(-PI/2,PI/2,nCoef,dxValues.elems,df1w);
		diCoefs.setSize(nCoef);
		getInterCoef(diCoefs.elems,nCoef,dxValues.elems,latValues.elems,lat0);
		for(k=0;k<nLats;k++)
		{
			double lat=(0.5*PI*k)/(nLats-1);
			_tprintf(_T("coef = %d, lat = %6.2f\n"),nCoef,lat*RAD_TO_DEG);
			double dxr=factor*df1w(lat);
			double dxi=factor*funcValue(lat,diCoefs.elems,nCoef,lat0);
			//_tprintf(_T("dx/da real    = %.12f m/rad\n"),dxr);
			//_tprintf(_T("dx/da inter   = %.12f m/rad\n"),dxi);
			diError[k]=dxi-dxr;
			//_tprintf(_T("inter  error  = %+.6e\n"),dxi-dxr);
			double xr=factor*RungeCutta4(lat,lat0,x0,nSteps,df1w);
			double xi=factor*integValue(lat,diCoefs.elems,nCoef,x0);
			//_tprintf(	_T("x runge  = %.12f m\n"),xr);
			//_tprintf( _T("x inter  = %.12f m\n"),xi);
			iError[k]=xi-xr;
		}
		double dem=mean(diError,nLats);
		double ded=sqrt(avar(diError,nLats));
		em[nCoef-nCoef0]=mean(iError,nLats);
		ed[nCoef-nCoef0]=sqrt(avar(iError,nLats));
		//_tprintf(_T("Inter derror mean = %+.6e, dev = %+.6e\n"),dem,ded);
		//_tprintf(_T("Inter error mean  = %+.6e, dev = %+.6e\n"),em,ed);
		if(minCoef<0||ABS(minError)>ABS(em[nCoef-nCoef0]))
		{
			minCoef=nCoef;
			minError=em[nCoef-nCoef0];
		}
		_TCHAR fileName[128];
		_stprintf(fileName,_T("coef%02d.txt"),nCoef);
		f=_tfopen(fileName,_T("w"));
		if(f)
		{
			_tprintf(_T("Writing coefs in __debug_out '%s'...\n"),fileName);
			_ftprintf(f,_T("Inter derror mean = %+.6e, dev = %+.6e\n"),dem,ded);
			_ftprintf(f,_T("Inter error mean  = %+.6e, dev = %+.6e\n"),em[nCoef],ed[nCoef]);
			for(k=0;k<nLats;k++)
			{
				double lat=(0.5*PI*k)/(nLats-1);
				_ftprintf(f,_T("lat = %6.2f:\n"),lat*RAD_TO_DEG);
				_ftprintf(f,_T("Inter derror  = %+.6e\n"),diError[k]);
				_ftprintf(f,_T("Inter error   = %+.6e\n"),iError[k]);
			}
			_ftprintf(f,_T("Inter dwef%d:\n"),nCoef);
			for(i=0;i<nCoef;i++)
				_ftprintf(f,_T("dcoef %02d = %+30.16le\n"),i,diCoefs.elems[i]);
			_ftprintf(f,_T("Inter wef%d:\n"),nCoef);
			_ftprintf(f,_T("icoef %02d = %+30.16le\n"),0,0.0);
			for(i=0;i<nCoef;i++)
				_ftprintf(f,_T("icoef %02d = %+30.16le\n"),i+1,diCoefs.elems[i]/(i+1));
			fclose(f);
		}
	}
	f=_tfopen(_T("err_wef_coef.txt"),_T("w"));
	if(f)
	{
		_ftprintf(f,_T("\nWEF: Test lats: %02d, Min mean: %+.6e, coefs = %02d\n"),nLats,minError,minCoef);
		for(nCoef=nCoef0;nCoef<=nCoefN;nCoef++)
			_ftprintf(f,_T("Coef %02d, error mean  = %+.6e, dev = %+.6e\n"),nCoef,em[nCoef-nCoef0],ed[nCoef-nCoef0]);
		fclose(f);
	}
}
void testInvPoly()
{
	int i=0,j=0,k=0;
	FILE *f=NULL;
	_TCHAR fileName[128];
	int nSteps=100000,nCoef;
	double lat0=0,x0=0,factor=ae*(1-e*e);
	//double lat1=1.5;
	//double lat2=Pinvwef17(RungeCutta4(lat1,lat0,x0,nSteps,df1w));
	//_tprintf(_T("lat = %f, Pinvwef17(Pwef17(lat)) = %f, error = %+0.6e\n"),lat1,lat2,lat2-lat1);
	//return;
	const int nLats=91,nCoefN=20,nCoef0=10;
	Vector<double> latValues,coefs,xValues;
	double minError=0;
	double em[nCoefN-nCoef0+1],ed[nCoefN-nCoef0+1];
	int minCoef=-1;
	double errors[nLats],x[nLats],latr[nLats];
	incData(0.0,PI/2,nLats,latr);
	_tprintf(_T("Start Position: %f, Integration Steps: %d, Latitude values: %d\n"),lat0,nSteps,nLats);
	for(j=0;j<nLats;j++)
	{
		_tprintf(_T("."));
		x[j]=RungeCutta4( latr[j],lat0,x0,nSteps,df1w);
	}
	_tprintf(	_T("OK\n"));
	//incData(0.0,PI/2,nLats,x,Pwef17);
	for(nCoef=nCoef0;nCoef<=nCoefN;nCoef++)
	{
		_tprintf(	_T("Calculating for nCoef = %d\n"),nCoef);
		latValues.setSize(nCoef);
		xValues.setSize(nCoef);
		incData(-PI/2,PI/2,nCoef,latValues.elems);
		for(j=0;j<nCoef;j++)
		{
			//xValues[j]=Pwef17(latValues[j]);
			xValues.elems[j]=RungeCutta4(latValues.elems[j],lat0,x0,nSteps,df1w);
			_tprintf(_T("."));
		}
		_tprintf(_T("OK\n"));
		coefs.setSize(nCoef);
		getInterCoef(coefs.elems,nCoef,latValues.elems,xValues.elems,x0);
		for(k=0;k<nLats;k++)
			errors[k]=funcValue(x[k],coefs.elems,nCoef,x0)-latr[k];
		em[nCoef-nCoef0]=mean((double*)errors,nCoef);
		ed[nCoef-nCoef0]=sqrt(avar((double*)errors,nCoef));
		if(minCoef<0||ABS(minError)>ABS(em[nCoef-nCoef0]))
		{
			minCoef=nCoef;
			minError=em[nCoef-nCoef0];
		}
		_tprintf(_T("Error mean  = %+.6e, dev = %+.6e\n"),em[nCoef-nCoef0],ed[nCoef-nCoef0]);
		_stprintf(fileName,_T("inv_coef%02d.txt"),nCoef);
		f=_tfopen(fileName,_T("w"));
		if(f)
		{
			_tprintf(_T("Writing coefs in __debug_out '%s'...\n"),fileName);
			_ftprintf(f,_T("Error mean  = %+.6e, dev = %+.6e\n"),em[nCoef-nCoef0],ed[nCoef-nCoef0]);
			for(k=0;k<nLats;k++)
			{
				_ftprintf(f,_T("latr = %17.12f deg\n"),latr[k]*RAD_TO_DEG);
				_ftprintf(f,_T("lati = %17.12f deg\n"),(latr[k]+errors[k])*RAD_TO_DEG);
				_ftprintf(f,_T("error = %+.6e\n"),errors[k]);
			}
			_ftprintf(f,_T("Inv wef%d:\n"),nCoef);
			for(i=0;i<nCoef;i++)
				_ftprintf(f,_T("coef %02d = %+30.16le\n"),i,coefs.elems[i]);
			fclose(f);
		}
	}
	f=_tfopen(_T("err_invwef_coef.txt"),_T("w"));
	if(f)
	{
		_ftprintf(f,_T("\nINVWEF: Test lats: %02d, Min mean: %+.6e, coefs = %02d\n"),nLats,minError,minCoef);
		for(nCoef=nCoef0;nCoef<=nCoefN;nCoef++)
			_ftprintf(f,_T("Coef %02d, error mean  = %+.6e, dev = %+.6e\n"),nCoef,em[nCoef-nCoef0],ed[nCoef-nCoef0]);
		fclose(f);
	}
}
void testRt()
{
	int i=0,j=0,k=0;
	FILE *f=NULL;
	_TCHAR fileName[128];
	int nCoefs;
	double lon0=0,y0=0,factor=ae*(1-e*e);
	const int nLons=30;
	Vector<double> lonValues,coefs,yValues;
	double errors[nLons],y[nLons],lonr[nLons];
	incData(0.0,PI/2,nLons,lonr);
	incData(0.0,PI/2,nLons,y,df2w);

	//incData(0.0,PI/2,nLons,y,Pwef17);
	for(nCoefs=10;nCoefs<=20;nCoefs++)
	{
		_tprintf(	_T("Calculoning for nCoefs = %d\n"),nCoefs);
		lonValues.setSize(nCoefs);
		yValues.setSize(nCoefs);
		incData(0.0,PI/2,nCoefs,lonValues.elems);
		incData(0.0,PI/2,nCoefs,yValues.elems,df2w);
		coefs.setSize(nCoefs);
		getInterCoef(coefs.elems,nCoefs,yValues.elems,lonValues.elems,y0);
		for(k=0;k<nLons;k++)
			errors[k]=funcValue(lonr[k],coefs.elems,nCoefs,y0)-y[k];
		double em=mean((double*)errors,nCoefs);
		double ed=sqrt(avar((double*)errors,nCoefs));
		_tprintf(_T("Error mean  = %+.6e, dev = %+.6e\n"),em,ed);
		_stprintf(fileName,_T("inv_coef%02d.tyt"),nCoefs);
		f=_tfopen(fileName,_T("w"));
		if(f)
		{
			_tprintf(_T("Writing coefs in __debug_out '%s'...\n"),fileName);
			_ftprintf(f,_T("Error mean  = %+.6e, dev = %+.6e\n"),em,ed);
			for(k=0;k<nLons;k++)
			{
				_ftprintf(f,_T("lonr = %17.12f deg\n"),lonr[k]*RAD_TO_DEG);
				_ftprintf(f,_T("loni = %17.12f deg\n"),(lonr[k]+errors[k])*RAD_TO_DEG);
				_ftprintf(f,_T("error = %+.6e\n"),errors[k]);
			}
			_ftprintf(f,_T("Inv wef%d:\n"),nCoefs);
			for(i=0;i<nCoefs;i++)
				_ftprintf(f,_T("coef %02d = %+30.16le\n"),i,coefs.elems[i]);
			fclose(f);
		}
	}
}
void testEuler()
{
	double e[3]={30*DEG_TO_RAD,30*DEG_TO_RAD,30*DEG_TO_RAD};
	double q[4],Rbn[3][3];
	EulerToRbn(e,Rbn);
	PRINT(_T("psi = %f, theta = %f, phi = %f\nR=\n"),e[0]*RAD_TO_DEG,e[1]*RAD_TO_DEG,e[2]*RAD_TO_DEG);
	printMatrix(__debug_out,Rbn);
	EulerToQuat(e,q);
	printQuat(__debug_out,q);
	quatToRbn(q,Rbn);
	printMatrix(__debug_out,Rbn,_T("Rbn=f(q)\n"));
}
void testRot()
{
	double Ren[3][3],Rne[3][3],Wie_e[3],Wie_n[3],lat,lon,Vn[3],Ve[3],out[3];
	lat=random(0,PI/2);
	lon=random(0,2*PI);
	randVector(Vn,0,1);
	printVector(__debug_out,Vn,_T("Vn = "),_T("\n"));
	GeoToRen(lat,lon,Ren);
	transMatrix(Ren,Rne);
	initVector(Wie_e,0.0,0.0,W_EARTH);
	mulVector(Ren,Wie_e,Wie_n);
	printVector(__debug_out,Wie_n,_T("Wie_n = "),_T("\n"));
	mulVector(Rne,Vn,Ve);
	printVector(__debug_out,Ve,_T("Ve = "),_T("\n"));
	cross(Wie_e,Ve,out);
	printVector(__debug_out,out,_T("Wie_e x Ve =           "),_T("\n"));
	mulVector(Ren,out);
	printVector(__debug_out,out,_T("Ren * ( Wie_e x Ve ) = "),_T("\n"));
	cross(Wie_n,Vn,out);
	printVector(__debug_out,out,_T("Wie_n x Vn =           "),_T("\n"));
}
void testQuat()
{
	double q[4],dq[4],euler[3]={DEG_TO_RAD*30,DEG_TO_RAD*30,DEG_TO_RAD*30},deuler[3]={0.000012,0,0.000012};
	EulerToQuat(euler,q);
	printQuat(__debug_out,q,NULL,_T("q = "),_T("\n"));
	EulerToQuat(deuler,dq);
	printQuat(__debug_out,dq,NULL,_T("dq = "),_T("\n"));
	mulQuat(dq,q);
	quatToEuler(q,euler);
	printVector(__debug_out,euler,NULL,_T("euler = "),_T("\n"));
	EulerToQuat(euler,q);
	double v[3]={1,0,0};
	double x=rotVector(q,v);
	printVector(__debug_out,v,NULL,_T("v = "),_T("\n"));
	PRINT(_T("x = %f\n"),x);
}
void testMatrix()
{
	const int cols=3,rows=4;
	double m5[2][2],m6[2][2],m7[2][2];
	Matrix m;
	Matrix m1(rows,cols);
	Matrix m2(m1);
	Matrix m3(cols,cols);
	Matrix m4(rows,rows);
	m1.rand();
	m2.rand();
	m3.rand();
	m4.clear();
	m1[0][0]=2;
	m2[0][0]=2;
	try
	{
		m1.print(__debug_out,_T("\nm1\n"));
		m2.print(__debug_out,_T("\nm2\n"));
		m3.print(__debug_out,_T("\nm3\n"));
		Matrix mT1=m1.trans();
		Matrix mT2=m2.trans();
		mT1.print(__debug_out,_T("\nm1'\n"));
		mT2.print(__debug_out,_T("\nm2'\n"));
		(2.0*m1-3.0*m2).print(__debug_out,_T("\n2.0*m1-3.0*m2\n"));
		(2.0*m1+3.0*m2).print(__debug_out,_T("\n2.0*m1+3.0*m2\n"));
		Matrix mR1=m2*mT1;
		Matrix mR2=mT2*m1;
		mR1.print(__debug_out,_T("\nm2*m1'\n"));
		mR2.print(__debug_out,_T("\nm2'*m1\n"));
		PRINT(_T("\nm2*m1.trans().det() = %f\n"),mR1.det());
		PRINT(_T("\nm2.trans()*m1.det() = %f\n"),mR2.det());
		
		Matrix mI2=mR2.inv();
		mI2.print(__debug_out,_T("\nm2.trans()*m1.inv()\n"));
		(mI2*mR2).print(__debug_out,_T("\ntest:\n"));
		
		Matrix mI1=mR1.inv(1e-9);
		mI1.print(__debug_out,_T("\nm2*m1.trans().inv()\n"));
		(mI1*mR1).print(__debug_out,_T("\ntest:\n"));
	}
	catch(int ex)
	{
		_tprintf(_T("Error matrix: %d\n"),ex);
		PRINT(_T("Error matrix: %d\n"),ex);
	}
	m.import(_T("1 2 3;4 5 6;7 8 9"),_T(" "),_T(";"));
	m.print(__debug_out);

	m=m1*m3*m1.trans();
	m.print(__debug_out,_T("\nm1*m3*m1'=\n"));
	mulMatrixTrans(m1,m3,m4,m1.rows,m1.cols);
	m4.print(__debug_out,_T("\nmulMatrixTrans(m1,m3)=\n"));
	(m-m4).print(__debug_out,_T("\ndiff=\n"));

	randMatrix(m5);
	randMatrix(m6);
	mulMatrixTrans(m5,m6,m7);
	printMatrix(__debug_out,m5,_T("\nm5 =["),_T(";"),_T(" %.9lf"),_T("];"));
	printMatrix(__debug_out,m6,_T("\nm6 =["),_T(";"),_T(" %.9lf"),_T("];"));
	printMatrix(__debug_out,m7,_T("\nm7a=["),_T(";"),_T(" %.9lf"),_T("];"));
	mulMatrix(m5,m6,m7);
	transMatrix(m5);
	mulMatrix(m7,m5,m6);
	printMatrix(__debug_out,m6,_T("\nm7b=["),_T(";"),_T(" %.9lf"),_T("];"));
}
void testNew()
{
	const int M=0x0fffffff;
	double *t;
	NEW_ARRAY(t,double,M);
	_tprintf(_T("t = 0x%08X\n"),t);
	DELETE_ARRAY(t);
}
void testMem()
{
	const int M=0x0fffffff;
	int *t1,*t2;
	NEW_ARRAY(t1,int,10);
	NEW_ARRAY(t2,int,5);
	_tprintf(_T("t1 = %08X\n"),t1);
	_tprintf(_T("t2 = %08X\n"),t2);
	_tprintf(_T("t2-t1 = %u\n"),(DWORD)t2-(DWORD)t1);
	DELETE_ARRAY(t1);
	DELETE_ARRAY(t2);
}
double testExpFunc(double x,int k)
{
	return exp(x);
}
void testExp()
{
	const int N=10;
	double x=random();
	double exp1=exp(x);
	_tprintf(_T("exp1 = %.12e\n"),exp1);
	double exp2=1;
	double f=1;
	for(int i=1;i<N;i++)
	{
		f*=i;
		exp2+=pown(x,i)/f;
	}
	double coefs[N],yValues[N],xValues[N];
	double h=INV(N-1);
	incData(0.0,1.0,N,xValues);
	incData(0.0,1.0,N,yValues,exp);
	getTaylorCoef(coefs,N,testExpFunc);
	double exp3=funcValue(x,coefs,N);
	getInterCoef(coefs,N,yValues,xValues);
	double exp4=funcValue(x,coefs,N);
	getInterCoef(coefs,N,yValues);
	double exp5=funcValue(x/h,coefs,N);
	_tprintf(_T("exp2 = %.12e\n"),exp2);
	_tprintf(_T("exp3 = %.12e\n"),exp3);
	_tprintf(_T("exp4 = %.12e\n"),exp4);
	_tprintf(_T("exp5 = %.12e\n"),exp5);
	_tprintf(_T("error exp2 = %+.12e\n"),exp2-exp1);
	_tprintf(_T("error exp3 = %+.12e\n"),exp3-exp1);
	_tprintf(_T("error exp4 = %+.12e\n"),exp4-exp1);
	_tprintf(_T("error exp5 = %+.12e\n"),exp5-exp1);
}
double testExpFunc2(double x)
{
	return exp(-0.5*x*x)/(SQRT2*SQRTPI);
}
double testExpFunc2diffk(double x,int k)
{
	if(k<0)
		return 0;
	if(k==0)
		return testExpFunc2(x);
	return -(k-1)*testExpFunc2diffk(x,k-2)-x*testExpFunc2diffk(x,k-1);
}
double expmean=0,expdev=1;
double testExpFunc2diff(double x,double y)
{
	return testExpFunc2((x-expmean)/expdev)/expdev;
}
void testExp2()
{
	const int N=14,S=10000;
	double x=0.1;
	_tprintf(_T("x = %.12e\n"),x);
	double exp1=RungeCutta4(x,0,0.5,S,testExpFunc2diff);
	_tprintf(_T("exp1 = %.12e\n"),exp1);
	double exp2=normalDistrib(x);//,expmean,expdev);
	_tprintf(_T("exp2 = %.12e\n"),exp2);
	//double coefs[2*N];
	//getTaylorCoef(testExpFunc2diffk,coefs,2*N);
	//double exp3=integValue(coefs,2*N,(x-expmean)/expdev)-integValue(coefs,2*N,0);
	//_tprintf(_T("exp3 = %.12e\n"),exp3);
	
	_tprintf(_T("error exp2 = %+.12e\n"),exp2-exp1);
}
void testNormalDistrib()
{
	double mean=0,sigma=1,arg,value;
	arg=sigma;
	value=normalDistrib2(-arg,arg,mean,sigma);
	_tprintf(_T("normDistrib(1 sigma) = %.2f, resting = %.2f\n"),100*value,100*(1-value));
	arg=2*sigma;
	value=normalDistrib2(-arg,arg,mean,sigma);
	_tprintf(_T("normDistrib(2 sigma) = %.2f, resting = %.2f\n"),100*value,100*(1-value));
	arg=3*sigma;
	value=normalDistrib2(-arg,arg,mean,sigma);
	_tprintf(_T("normDistrib(3 sigma) = %.2f, resting = %.2f\n"),100*value,100*(1-value));
}
void testRandom()
{
	const int N=1000000,H=60;
	double s=H/6.0,m=H/2.0;
	Stat normalStat,uniStat;
	PrecisionTimer timer;
	for(int i=0;i<N;i++)
	{
		normalStat.add(random_n(s,m));
		uniStat.add(random_u(s,m));
	}
	normalStat.update();
	uniStat.update();
	_tprintf(_T("\nRAND TIME: %f s\n"), timer.time());
	_tprintf(_T("\nNORMAL :\n\t mean = %e\n\t  dev = %e\n\t skew = %e\n\texcess = %e\n"),normalStat.mean,normalStat.stdev,normalStat.skew,normalStat.excess);
	_tprintf(_T("\nUNIFORM:\n\t mean = %e\n\t  dev = %e\n\t skew = %e\n\texcess = %e\n"),uniStat.mean,uniStat.stdev,uniStat.skew,uniStat.excess);
}
void testRandom3d()
{
	const int N=100000;
	const double c[3]={random(-0.5,0.5),random(-0.5,0.5),random(-0.5,0.5)};
	int i;
	Table<double> n(3,N);
	double v[3],cc[3];
	if(!random_n3d(v,c))
	{
		_tprintf(_T("correl error!\n"));
		return;
	}
	n[0][0]=v[0];
	n[1][0]=v[1];
	n[2][0]=v[2];
	for(i=1;i<N;i++)
	{
		random_n3d(v,c);
		n[0][i]=v[0];
		n[1][i]=v[1];
		n[2][i]=v[2];
	}
	cc[0]=correl(n[0],n[1],N);
	cc[1]=correl(n[1],n[2],N);
	cc[2]=correl(n[2],n[0],N);
	_tprintf(_T("Real: \n"));
	_tprintf(_T("xy=%f\n"),c[0]);
	_tprintf(_T("yz=%f\n"),c[1]);
	_tprintf(_T("zx=%f\n"),c[2]);
	_tprintf(_T("Calc: \n"));
	_tprintf(_T("xy=%f\n"),cc[0]);
	_tprintf(_T("yz=%f\n"),cc[1]);
	_tprintf(_T("zx=%f\n"),cc[2]);
}
void testString()
{
	CSTR s1=_T("<Test..."),s2=_T("ha ha ha>");
	String s(256);

	s=s1;
	_tprintf(_T("%s, %d\n"),(STR)s, s==s1);

	s+=s2; 
	_tprintf(_T("%s, %d\n"),(STR)s, s==s2);

	_tprintf(_T("%s\n"),(CSTR)("start "+s+" middle "+s+" end"));

	int l=s.insert("1234 ");
	_tprintf(_T("%s\n"),(CSTR)s);

	s.insert("5678 ",l);
	_tprintf(_T("%s\n"),(CSTR)s);

	_tprintf(_T("%s\n"),s.print("%d %.2f",1234, 3.14f));

	_tprintf(_T("%d\n"),s.indexOf("3.14",1));

	s.remove(5,-1);
	_tprintf(_T("%s\n"),(CSTR)s);

	s.remove(0);
	_tprintf(_T("%s\n"),(CSTR)s);

	_tprintf(_T("%c, %c\n"),s[0],*s);
}
void testLSM()
{
	const int M=3,N=3;
	double	a[M],
			y[N]={ 1, 0, 1},
			x[N]={-1, 0, 1};
	if(getLSMPolyCoef(a,M,y,x,N))
	{
		_tprintf(_T("\nPolynomial:\n"));
		if(M>0)
			_tprintf(_T("(%f)"),a[0]);
		if(M>1)
			_tprintf(_T("+(%f)x"),a[1]);
		for(int i=2;i<M;i++)
			_tprintf(_T("+(%f)x^%d"),a[i],i);
	}
	else
	{
		_tprintf(_T("error!"));
	}
	_tprintf(_T("\n"));
}
template <typename T>
class Vector4
{
public:
	T x,y,z,l;

	// Constructs and sets the vector to (0, 0, 0)
	Vector4() : x(0), y(0), z(0), l(0) { }

	// Constructs and sets the vector to (vx, vy, vz)
	Vector4(T vx, T vy, T vz, T vw) : x(vx), y(vy), z(vz), l(vw) { }

	Vector4(const T *v) : x(v[0]), y(v[1]), z(v[2]), l(v[3]) { }

	// Constructs with data from another vector
	Vector4(const Vector4<T>& v) : x(v.x), y(v.y), z(v.z), l(v.l) { }

	~Vector4() { }

	Vector4<T> operator-(const Vector4<T>& v) const
	{
		return Vector4<T>(x - v.x, y - v.y, z - v.z, l - v.l);
	}

	bool operator==(const Vector4<T>& v) const
	{
		return x == v.x && y == v.y && z == v.z && l == v.l;
	}

	const Vector4<T>& operator=(const Vector4<T>& v)
	{
		x = v.x;
		y = v.y;
		z = v.z;
		l = v.l;
		return *this;
	}

	const Vector4<T>& operator=(const T *v)
	{
		x = v[0];
		y = v[1];
		z = v[2];
		l = v[3];
		return *this;
	}

	void set(T vx, T vy, T vz, T vw) {x = vx; y = vy; z = vz; l = vw;}
};
template <typename T>
inline void v_diff_inline(const Vector4<T> &v1, const Vector4<T> &v2, Vector4<T> &v)
{
	v.x = v1.x - v2.x;
	v.y = v1.y - v2.y;
	v.y = v1.y - v2.y;
	v.l = v1.l - v2.l;
}
template <typename T>
inline bool v_eq_inline(const Vector4<T> &v1, const Vector4<T> &v2)
{
	return v1.x == v2.x && v1.y == v2.y && v1.z == v2.z && v1.l == v2.l;
}
template <typename T>
inline bool v_eq_mem(const Vector4<T> &v1, const Vector4<T> &v2)
{
	return memcmp(&v1,&v2,sizeof(Vector4<T>))==0;
}
typedef Vector4<float> Vector4f;
void testSpeed()
{
	register int i;
	const static int iters=10000;
	PrecisionTimer timer;
	double time1,time2,time3,time4;
	
	Vector<Vector4f> buffer1;
	Vector4f *particle;
	Vector<int>buffer2;
	int *result;
	particle = buffer1.setSize(iters);
	result = buffer2.setSize(iters);
	if(!particle||!result)
	{
		_tprintf(_T("memory error\n"));
		return;
	}

	Sleep(0);
	timer.restart();
	for(i=0;i<iters;i++)
	{
		particle[i].set((float)random(),(float)random(),(float)random(),(float)random());
	}
	time1=timer.restart();

	Sleep(0);
	timer.restart();
	for(i=0;i<iters-1;i++)
	{
		result[i] = particle[i+1] == particle[i];
	}
	time2=timer.restart();
	
	Sleep(0);
	timer.restart();
	for(i=0;i<iters-1;i++)
	{
		result[i] = v_eq_inline(particle[i+1], particle[i]);
	}
	time3=timer.restart();

	Sleep(0);
	timer.restart();
	for(i=0;i<iters-1;i++)
	{
		result[i] = v_eq_mem(particle[i+1], particle[i]);
	}
	time4=timer.restart();

	_tprintf(_T("data generation time = %.3f ms\n"),1e3*time1);
	_tprintf(_T("class test time = %.3f ms\n"),1e3*time2);
	_tprintf(_T("inline test time = %.3f ms\n"),1e3*time3);
	_tprintf(_T("mem test time = %.3f ms\n"),1e3*time4);
}
void showWGS84()
{
	_tprintf(_T("WGS84:\n"));
	_tprintf(_T("A  = %20.10lf\n"),A_EARTH);
	_tprintf(_T("F  = %20.10lf\n"),F_EARTH);
	_tprintf(_T("B  = %20.10lf\n"),A_EARTH*(1-F_EARTH));
	_tprintf(_T("K  = %20.10lf\n"),1-F_EARTH);
	_tprintf(_T("E  = %20.10lf\n"),sqrt(1-pow2(1-F_EARTH)));
	_tprintf(_T("E2 = %20.10lf\n"),1-pow2(1-F_EARTH));
}
void testECEFtoLLA()
{
	showWGS84();
	double ecef[3],lla1[3],lla[3]={DEG_TO_RAD*80,DEG_TO_RAD*70,500};
	_tprintf(_T("lat = %f, lon = %f, alt = %f\n"),RAD_TO_DEG*lla[0],RAD_TO_DEG*lla[1],lla[2]);
	LLAtoECEF(lla,ecef);
	_tprintf(_T("x = %f, y = %f, z = %f\n"),ecef[0],ecef[1],ecef[2]);
	for(int i=0;i<3;i++)
	{
		ECEFtoLLA(ecef,lla1,i);
		_tprintf(_T("\ntest %d\n"),i);
		_tprintf(_T("lat = %f, lon = %f, alt = %f\n"),RAD_TO_DEG*lla1[0],RAD_TO_DEG*lla1[1],lla1[2]);
		_tprintf(_T("dlat = %e, dlon = %e, dalt = %e\n"),RAD_TO_DEG*(lla1[0]-lla[0]),RAD_TO_DEG*(lla1[1]-lla[1]),lla1[2]-lla[2]);
	}
}
void testWEF()
{
	//showWGS84();
	double lat0,lat,deg_lat=90,x=0;
	lat0=lat=DEG_TO_RAD*deg_lat;
	x=wef(lat);
	_tprintf(_T("wef(%f)=%f\n"),RAD_TO_DEG*lat,x);
	lat=invwef(x);
	_tprintf(_T("invwef(%f)=%f\n"),x,RAD_TO_DEG*lat);
	_tprintf(_T("error=%e deg\n"),RAD_TO_DEG*(lat-lat0));
}
void testInitInfo()
{
	InitInfo info,info2;
	info.add(_T("kolio_gilana"),_T("pi4aga"));
	info.add(_T("boi4o_borisov"),_T("prostak"));
	info.write(_T("info.txt"));
	info2.read(_T("info.txt"));
	info2.write(stdout);
}
class TT
{
	int t1;
	char t2;
	TT():t1(2),t2('c'){}
};
void testUDP()
{
	const int N=72;
	char dataSend[N]="123456789\0",dataRecv[N];


	Socket udp1,udp2;
	if(udp1.connect("localhost",1500,3000)&&udp2.connect("localhost",3000,1500))
	{
		printf("Connection successful.\n");
		udp1.send(dataSend,N);
		printf("SEND 1->2 %d bytes>> data: %s, error: %5d\n",N,dataSend,udp1.error);
		udp2.receive(dataRecv,N);
		printf("RECV 1->2 %d bytes>> data: %s, error: %5d\n",N,dataRecv,udp2.error);
		udp2.send(dataSend,N);
		printf("SEND 2->1 %d bytes>> data: %s, error: %5d\n",N,dataSend,udp1.error);
		udp1.receive(dataRecv,N);
		printf("RECV 2->1 %d bytes>> data: %s, error: %5d\n",N,dataRecv,udp2.error);
	}
}

void testXPlane()
{
	const int N=72;
	#pragma pack(push,1) // turn off data boundary alignment
	struct DataStruct
	{
		ULONG loops;
		float runtime;
		float sample;
		float a[3];
		float rot[3];
		float lla[3];
		float v[3];
		float euler[3];
	} data;
	#pragma pack(pop)


	Socket udp;
	if(udp.connect("localhost",1500,3000))
	{
		for(int i=0;i<20;i++)
		{
			printf("Connection successful.\n");
			udp.receive((char *)&data,sizeof(data));
			printf("RECV %d bytes, error: %5d\n",sizeof(data),udp.error);
			printf("loops = %d\nruntime = %f\nsample = %f\n",data.loops,data.runtime,data.sample);
		}
	}
}
/*
void testRNG()
{
	PrecisionTimer timer;
	Random r((ULONG)time(NULL));
	Stat stat;
	register int i;
	const int I=3000000;
	printf("I: %.0e\n",double(I));

	timer.restart();
	stat.clear();
	for(i=0;i<I;i++)
		stat.add(random());
	printf("\n     RANDOM    t=%6dms, m = %+.4lf, d = %.4lf, s = %+.4lf, e = %+.4lf\n",round(1e3*timer.time()),stat.mean(),stat.stdev(),stat.simetry(),stat.excess());

	timer.restart();
	stat.clear();
	for(i=0;i<I;i++)
		stat.add(r.uniform());
	printf("   R RANDOM    t=%6dms, m = %+.4lf, d = %.4lf, s = %+.4lf, e = %+.4lf\n",round(1e3*timer.time()),stat.mean(),stat.stdev(),stat.simetry(),stat.excess());

	timer.restart();
	stat.clear();
	for(i=0;i<I;i++)
		stat.add(random_n());
	printf("\n     RANDOM_N  t=%6dms, m = %+.4lf, d = %.4lf, s = %+.4lf, e = %+.4lf\n",round(1e3*timer.time()),stat.mean(),stat.stdev(),stat.simetry(),stat.excess());
	
	timer.restart();
	stat.clear();
	for(i=0;i<I;i++)
		stat.add(r.normal());
	printf("   R RANDOM_N  t=%6dms, m = %+.4lf, d = %.4lf, s = %+.4lf, e = %+.4lf\n",round(1e3*timer.time()),stat.mean(),stat.stdev(),stat.simetry(),stat.excess());
}
*/
void testCorrel()
{
	PrecisionTimer timer;
	const int N=1000000;
	double r1,r2,corr,m1,m2,v1,v2,v12,s1,s2,c;
	register int i,j;
	for(j=0;j<=10;j++)
	{
		corr=j*0.1;
		for(m1=0,m2=0,v1=0,v2=0,v12=0,i=0;i<N;i++)
		{
			random_n(&r1,&r2,corr,3,-0.55);
			v12+=r1*r2;
			v1+=r1*r1;
			v2+=r2*r2;
			m1+=r1;
			m2+=r2;
		}
		m1/=N;
		m2/=N;
		v12=v12/N-m1*m2;
		s1=sqrt(v1/N-m1*m1);
		s2=sqrt(v2/N-m2*m2);
		c=v12/(s1*s2);
		printf("corrin=%+.2f, corrout=%+.3f, m1=%+.2f, m2=%+.2f, s1=%.2f, s2=%.2f\n",corr,c,m1,m2,s1,s2);
	}
}
void testVarProduct()
{
	PrecisionTimer timer;
	const int N=10000000;
	double v1,v2,v12;
	register int i,j;
	for(j=0;j<=10;j++)
	{
		double s1 = random(0.5, 2), s2 = random(0.5, 2);
		for(v1=0,v2=0,v12=0,i=0;i<N;i++)
		{
			double r1,r2;
			random_n(&r1,&r2,0,s1,s2);
			v12+=pow2(r1*r2);
			v1+=r1*r1;
			v2+=r2*r2;
		}
		v12=v12/N;
		v1=v1/N;
		v2=v2/N;
		printf("v12=%.3f, v1*v2=%.3f, s1=%.3f, s2=%.3f\n",v12,v1*v2,s1,s2);
	}
}
void testMulError()
{
	const int N=1000000;
	double r1,r2,f,m1=random(0,5),m2=random(0,5),s1=random(0,2),s2=random(0,2),c12=random(-1,1),m=0,s=0;
	register int i;
	// test a*b
	m=m1*m2+c12*s1*s2;
	s=sqrt(pow2(m2*s1)+pow2(m1*s2)+2*c12*m1*m2*s1*s2+pow2(s1*s2)*(1+pow2(c12)));
	printf("ANALITIC1:\t s=%+.6f, m=%+.6f\n",s,m);
	m=m1*m2;
	s=sqrt(pow2(m2*s1)+pow2(m1*s2)+2*c12*m1*m2*s1*s2);
	printf("ANALITIC2:\t s=%+.6f, m=%+.6f\n ",s,m);
	for(i=0;i<N;i++)
	{
		random_n(&r1,&r2,c12,s1,s2,m1,m2);
		f=r1*r2;
		m+=f;
		s+=f*f;
	}
	m/=N;
	s=sqrt(s/N-m*m);
	printf(" NUMERIC:\t s=%+.6f, m=%+.6f\n",s,m);
}
void testLengthError()
{
	const int N=1000000;
	double	x,y,f,m=0,s=0,
			mx=7,my=4,sx=0.5,sy=0.5,cxy=-1;
	register int i;
	m=sqrt(mx*mx+my*my+sx*sx+sy*sy);
	s=sqrt((mx*mx*sx*sx+my*my*sy*sy+cxy*mx*my*sx*sy)/(mx*mx+my*my));
	printf("sa=%+.4f, ma=%+.4f\n",s,m);
	for(i=0;i<N;i++)
	{
		random_n(&x,&y,cxy,sx,sy,mx,my);
		f=getLength(x,y);
		m+=f;
		s+=f*f;
	}
	m/=N;
	s=sqrt(s/N-m*m);
	printf("sn=%+.4f, mn=%+.4f\n",s,m);
}
void testOrientError()
{
	// x=(xk-xi)/||xk-xi||
	const int N=1000000;
	double	x,y,mx=0,my=0,sx=0,sy=0,cxy=0,
			xk,yk,mxk=random(0,1),myk=random(0,1),sxk=random(0,1),syk=random(0,1),cxyk=random(-1,1), // R
			xi,yi,mxi=random(0,1),myi=random(0,1),sxi=random(0,1),syi=random(0,1),cxyi=random(-1,1), // Ri
			l=0,ml=random(0,1),sl=random(0,1), // li 
			d,md;
	register int i;
	md=getLength(mxk-mxi,myk-myi);
	mx=(mxk-mxi)/md;
	my=(myk-myi)/md;
	printf("ANALITIC:\nmd=%+.4f, mx=%+.4f, my=%+.4f, sx=%.4f, sy=%.4f, cxy=%+.4f\n",md,mx,my,sx,sy,cxy);
	for(i=0;i<N;i++)
	{
		random_n(&xk,&yk,cxyk,sxk,syk,mxk,myk); // R
		random_n(&xi,&yi,cxyi,sxi,syi,mxi,myi); // Ri
		md+=d=getLength(xk-xi,yk-yi);
		mx+=x=(xk-xi)/d;
		my+=y=(yk-yi)/d;
		sx+=x*x;
		sy+=y*y;
		cxy+=x*y;
	}
	md/=N;
	mx/=N;
	my/=N;
	sx=sqrt(sx/N-mx*mx);
	sy=sqrt(sy/N-my*my);
	cxy=(cxy/N-mx*my)/(sx*sy);
	printf("NUMERIC:\nmd=%+.4f, mx=%+.4f, my=%+.4f, sx=%.4f, sy=%.4f, cxy=%+.4f\n",md,mx,my,sx,sy,cxy);
}
void testArraySpeed()
{
	PrecisionTimer timer;
	const int M=100000,N=1000;
	register int i,j;
	double sa[N],*da;

	Vector<double> a;
	List<double> a2;
	ListObject<double> *d;
	BOOL ok1,ok2;
	
	NEW_ARRAY(da,double,N);

	timer.restart();
	ok1=!!a.setSize(N);
	printf("StatArray alloc time = %.2fms\n",1000*timer.time());
	
	timer.restart();
	ok2=!!a2.setSize(N);
	printf("DynArray alloc time = %.2fms\n",1000*timer.time());
	
	if(ok1&&ok2)
	{
		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				da[i]=0;
		printf("double* time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				sa[i]=0;
		printf("double[] time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				a.elems[i]=0;
		printf("StatArray time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(d=a2.first;d;d=d->next)
				d->value=0;
		printf("DynArray time = %.2fms\n",1000*timer.time());
	}
	DELETE_ARRAY(da);
}
void testTableSpeed()
{
	PrecisionTimer timer;
	const int M=100,N=1000;
	register int i,j,k;
	double **da,*da_uni,tmp;
	Table<double> t;
	Vector<double> a;
	Matrix m;
	BOOL ok1=FALSE,ok2=FALSE,ok3=FALSE;

	timer.restart();
	ok1=!!a.setSize(N*N);
	da_uni=a.elems;
	printf("array alloc time = %.2fms\n",1000*timer.time());
	
	timer.restart();
	ok2=!!t.setSize(N,N);
	da=t.elems;
	printf("table alloc time = %.2fms\n",1000*timer.time());

	timer.restart();
	ok3=!!m.setSize(N,N);
	printf("matrix alloc time = %.2fms\n",1000*timer.time());
	
	if(ok1&&ok2&&ok3)
	{
		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				for(k=0;k<N;k++)
					tmp=0;
		printf("operation set time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				for(k=0;k<N;k++)
					da[i][k]=0;
		printf("direct index time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				for(k=0;k<N;k++)
					da_uni[i+k*N]=0;
		printf("compute index time = %.2fms\n",1000*timer.time());

		timer.restart();
		for(j=0;j<M;j++)
			for(i=0;i<N;i++)
				for(k=0;k<N;k++)
					m[i][k]=0;
		printf("operator index time = %.2fms\n",1000*timer.time());
	}
}
void testColor()
{
	RGBAComp *rgbComp;
	BYTE *rgb,hslb[3],rgbb[3];
	float rgbf[3],hsl[3];
	DWORD color=colorByteToIntA(0,0,255,255);
	rgbComp=(RGBAComp *)&color;
	rgb=INT_TO_BYTE(color);
	printf("redComp = %d, greenComp = %d, blueComp = %d, alphaComp = %d\n",rgbComp->red,rgbComp->green,rgbComp->blue,rgbComp->alpha);
	printf("red = %d, green = %d, blue = %d, alpha = %d\n",rgb[RED],rgb[GREEN],rgb[BLUE],rgb[ALPHA]);
	printf("redFloat = %.3f, greenFloat = %.3f, blueFloat = %.3f, alphaFloat = %.3f\n",BYTE_TO_FLOAT(rgb[RED]),BYTE_TO_FLOAT(rgb[GREEN]),BYTE_TO_FLOAT(rgb[BLUE]),BYTE_TO_FLOAT(rgb[ALPHA]));
	rgb=INT_TO_BYTE(color);
	colorByteToFloat(rgb,rgbf);
	colorRGBtoHSL(rgbf,hsl);
	colorFloatToByte(hsl,hslb);
	colorHSLtoRGB(hsl,rgbf);
	colorFloatToByte(rgbf,rgbb);
	printf("hue = %d, sat = %d, lum = %d\n",hslb[HUE],hslb[SAT],hslb[LUM]);
	printf("red = %d, green = %d, blue = %d\n",rgbb[RED],rgbb[GREEN],rgbb[BLUE]);
	colorHueToRGB(0.5,rgbf);
	colorFloatToByte(rgbf,rgbb);
	printf("red = %d, green = %d, blue = %d\n",rgbb[RED],rgbb[GREEN],rgbb[BLUE]);
}
void testTrilat3D()
{
	const int NMAX=20,NMIN=5;
	const double S=1,L=100,EPS=0;
	int i,j,k,N;
	double	xi[NMAX],yi[NMAX],zi[NMAX],l[NMAX],R[NMAX],lij,lik,sumx,sumy,sumz,suml,sumb,x,y,z,ex,ey,ez,e,m;
	Matrix A,P,B,At,AtA,AtAinv,Apseudo;

	x=L*random();
	y=L*random();
	z=L*random();

	PRINT(_T("POINT: x=%lf,  y=%lf, z=%lf\n"),x,y,z);
	_ftprintf(stdout,_T("POINT: x=%lf,  y=%lf, z=%lf\n"),x,y,z);
	for(i=0;i<NMAX;i++)
	{
		xi[i]=L*random();
		yi[i]=L*random();
		zi[i]=L*random();
		R[i]=getLength(xi[i],yi[i],zi[i]);
		l[i]=getLength(xi[i]-x+random_n(S),yi[i]-y+random_n(S),zi[i]-z+random_n(S))+random_n(S);
	}


	for(N=NMIN;N<=NMAX;N++)
	{	
		PRINT(_T("\n************************* N=%d ***************************\n\n"),N);
		_ftprintf(stdout,_T("\n************************* N=%d ***************************\n\n"),N);
		PRINT(_T("CASE 1 TOF1:\n"));
		_ftprintf(stdout,_T("CASE 1 TOF1: "));
		A.setSize(N-1,3);
		P.setSize(3,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[0]-xi[i+1];
			A.elems[i][1]=yi[0]-yi[i+1];
			A.elems[i][2]=zi[0]-zi[i+1];
			B.elems[i][0]=0.5*(pow2(R[0])-pow2(l[0])-pow2(R[i+1])+pow2(l[i+1]));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}
		

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 2 TOF2:\n"));
		_ftprintf(stdout,_T("CASE 2 TOF2: "));
		A.setSize(N-1,3);
		P.setSize(3,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[i]-xi[i+1];
			A.elems[i][1]=yi[i]-yi[i+1];
			A.elems[i][2]=zi[i]-zi[i+1];
			B.elems[i][0]=0.5*(pow2(R[i])-pow2(l[i])-pow2(R[i+1])+pow2(l[i+1]));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 3 sTOF:\n"));
		_ftprintf(stdout,_T("CASE 3 sTOF: "));
		A.setSize(N,3);
		P.setSize(3,1);
		B.setSize(N,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[i]-xi[i+1];
			A.elems[i][1]=yi[i]-yi[i+1];
			A.elems[i][2]=zi[i]-zi[i+1];
			B.elems[i][0]=0.5*(pow2(R[i])-pow2(l[i])-pow2(R[i+1])+pow2(l[i+1]));
		}
		A.elems[N-1][0]=xi[N-1]-xi[0];
		A.elems[N-1][1]=yi[N-1]-yi[0];
		A.elems[N-1][2]=zi[N-1]-zi[0];
		B.elems[N-1][0]=0.5*(pow2(R[N-1])-pow2(l[N-1])-pow2(R[0])+pow2(l[0]));

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 4 DTOA1:\n"));
		_ftprintf(stdout,_T("CASE 4 DTOA1: "));
		A.setSize(N-1,4);
		P.setSize(4,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			lij=l[0]-l[i+1];
			A.elems[i][0]=xi[0]-xi[i+1];
			A.elems[i][1]=yi[0]-yi[i+1];
			A.elems[i][2]=zi[0]-zi[i+1];
			A.elems[i][3]=lij;
			B.elems[i][0]=0.5*(pow2(R[0])-pow2(R[i+1])+pow2(lij));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 5 DTOA2:\n"));
		_ftprintf(stdout,_T("CASE 5 DTOA2: "));
		A.setSize(N-1,3);
		P.setSize(3,1);
		B.setSize(N-1,1);
		suml=sumb=sumx=sumy=sumz=0;
		for(i=0;i<N-1;i++)
		{
			suml+=lij=l[0]-l[i+1];
			sumx+=A.elems[i][0]=xi[0]-xi[i+1];
			sumy+=A.elems[i][1]=yi[0]-yi[i+1];
			sumz+=A.elems[i][2]=zi[0]-zi[i+1];
			sumb+=B.elems[i][0]=0.5*(pow2(R[0])-pow2(R[i+1])+pow2(lij));
		}
		for(i=0;i<N-1;i++)
		{
			m=-(l[0]-l[i+1])/suml;
			A.elems[i][0]+=m*sumx;
			A.elems[i][1]+=m*sumy;
			A.elems[i][2]+=m*sumz;
			B.elems[i][0]+=m*sumb;
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 6 DTOA3:\n"));
		_ftprintf(stdout,_T("CASE 6 DTOA3: "));
		A.setSize(N-2,3);
		P.setSize(3,1);
		B.setSize(N-2,1);
		for(i=0;i<N-2;i++)
		{
			j=i+1;
			k=i+2;
			lij=l[0]-l[i+1];
			lik=l[0]-l[i+2];
			A.elems[i][0]=(xi[0]-xi[i+1])*lik-(xi[0]-xi[i+2])*lij;
			A.elems[i][1]=(yi[0]-yi[i+1])*lik-(yi[0]-yi[i+2])*lij;
			A.elems[i][2]=(zi[0]-zi[i+1])*lik-(zi[0]-zi[i+2])*lij;
			B.elems[i][0]=0.5*((pow2(R[0])-pow2(R[i+1])+pow2(lij))*lik-(pow2(R[0])-pow2(R[i+2])+pow2(lik))*lij);
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 7 sDTOA:\n"));
		_ftprintf(stdout,_T("CASE 7 sDTOA: "));
		A.setSize(N,3);
		P.setSize(3,1);
		B.setSize(N,1);
		i=0;
		j=1;
		k=N-1;
		lij=l[i]-l[j];
		lik=l[i]-l[k];
		A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
		A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
		A.elems[i][2]=(zi[i]-zi[j])*lik-(zi[i]-zi[k])*lij;
		B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);
		for(i=1;i<N-1;i++)
		{
			j=i+1;
			k=i-1;
			lij=l[i]-l[j];
			lik=l[i]-l[k];
			A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
			A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
			A.elems[i][2]=(zi[i]-zi[j])*lik-(zi[i]-zi[k])*lij;
			B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);
		}
		i=N-1;
		j=0;
		k=N-2;
		lij=l[i]-l[j];
		lik=l[i]-l[k];
		A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
		A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
		A.elems[i][2]=(zi[i]-zi[j])*lik-(zi[i]-zi[k])*lij;
		B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			ez=P.elems[2][0];
			e=getLength(ex-x,ey-y,ez-z);
			PRINT(_T("\nx=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf, z=%lf, error=%le\n"),ex,ey,ez,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}
	}
}

void testTrilat2D()
{
	const int NMAX=16,NMIN=4;
	const double S=0.01,L=1,EPS=0;
	int i,j,k,N;
	double	xi[NMAX],yi[NMAX],l[NMAX],R[NMAX],lij,lik,sumx,sumy,suml,sumb,x,y,ex,ey,e,m;
	Matrix A,P,B,At,AtA,AtAinv,Apseudo;

	x=L*random();
	y=L*random();
	PRINT(_T("POINT: x=%lf,  y=%lf\n"),x,y);
	_ftprintf(stdout,_T("POINT: x=%lf,  y=%lf\n"),x,y);
	for(i=0;i<NMAX;i++)
	{
		xi[i]=L*random();
		yi[i]=L*random();
		R[i]=getLength(xi[i],yi[i]);
		l[i]=getLength(xi[i]-x+random_n(S),yi[i]-y+random_n(S))+random_n(S);
	}

	for(N=NMIN;N<=NMAX;N++)
	{
		PRINT(_T("\n************************* N=%d ***************************\n\n"),N);
		_ftprintf(stdout,_T("\n************************* N=%d ***************************\n\n"),N);
		PRINT(_T("CASE 1 TOF1:\n"));
		_ftprintf(stdout,_T("CASE 1 TOF1: "));
		A.setSize(N-1,2);
		P.setSize(2,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[0]-xi[i+1];
			A.elems[i][1]=yi[0]-yi[i+1];
			B.elems[i][0]=0.5*(pow2(R[0])-pow2(l[0])-pow2(R[i+1])+pow2(l[i+1]));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}
		

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 2 TOF2:\n"));
		_ftprintf(stdout,_T("CASE 2 TOF2: "));
		A.setSize(N-1,2);
		P.setSize(2,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[i]-xi[i+1];
			A.elems[i][1]=yi[i]-yi[i+1];
			B.elems[i][0]=0.5*(pow2(R[i])-pow2(l[i])-pow2(R[i+1])+pow2(l[i+1]));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 3 sTOF:\n"));
		_ftprintf(stdout,_T("CASE 3 sTOF: "));
		A.setSize(N,2);
		P.setSize(2,1);
		B.setSize(N,1);
		for(i=0;i<N-1;i++)
		{
			A.elems[i][0]=xi[i]-xi[i+1];
			A.elems[i][1]=yi[i]-yi[i+1];
			B.elems[i][0]=0.5*(pow2(R[i])-pow2(l[i])-pow2(R[i+1])+pow2(l[i+1]));
		}
		A.elems[N-1][0]=xi[N-1]-xi[0];
		A.elems[N-1][1]=yi[N-1]-yi[0];
		B.elems[N-1][0]=0.5*(pow2(R[N-1])-pow2(l[N-1])-pow2(R[0])+pow2(l[0]));

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 4 DTOA1:\n"));
		_ftprintf(stdout,_T("CASE 4 DTOA1: "));
		A.setSize(N-1,3);
		P.setSize(3,1);
		B.setSize(N-1,1);
		for(i=0;i<N-1;i++)
		{
			lij=l[0]-l[i+1];
			A.elems[i][0]=xi[0]-xi[i+1];
			A.elems[i][1]=yi[0]-yi[i+1];
			A.elems[i][2]=lij;
			B.elems[i][0]=0.5*(pow2(R[0])-pow2(R[i+1])+pow2(lij));
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;
			
			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 5 DTOA2:\n"));
		_ftprintf(stdout,_T("CASE 5 DTOA2: "));
		A.setSize(N-1,2);
		P.setSize(2,1);
		B.setSize(N-1,1);
		suml=sumb=sumx=sumy=0;
		for(i=0;i<N-1;i++)
		{
			suml+=lij=l[0]-l[i+1];
			sumx+=A.elems[i][0]=xi[0]-xi[i+1];
			sumy+=A.elems[i][1]=yi[0]-yi[i+1];
			sumb+=B.elems[i][0]=0.5*(pow2(R[0])-pow2(R[i+1])+pow2(lij));
		}
		for(i=0;i<N-1;i++)
		{
			m=-(l[0]-l[i+1])/suml;
			A.elems[i][0]+=m*sumx;
			A.elems[i][1]+=m*sumy;
			B.elems[i][0]+=m*sumb;
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 6 DTOA3:\n"));
		_ftprintf(stdout,_T("CASE 6 DTOA3: "));
		A.setSize(N-2,2);
		P.setSize(2,1);
		B.setSize(N-2,1);
		for(i=0;i<N-2;i++)
		{
			j=i+1;
			k=i+2;
			lij=l[0]-l[i+1];
			lik=l[0]-l[i+2];
			A.elems[i][0]=(xi[0]-xi[i+1])*lik-(xi[0]-xi[i+2])*lij;
			A.elems[i][1]=(yi[0]-yi[i+1])*lik-(yi[0]-yi[i+2])*lij;
			B.elems[i][0]=0.5*((pow2(R[0])-pow2(R[i+1])+pow2(lij))*lik-(pow2(R[0])-pow2(R[i+2])+pow2(lik))*lij);
		}

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}

		PRINT(_T("\n///////////////////////////////\n\n"));

		PRINT(_T("CASE 7 sDTOA:\n"));
		_ftprintf(stdout,_T("CASE 7 sDTOA: "));
		A.setSize(N,2);
		P.setSize(2,1);
		B.setSize(N,1);
		i=0;
		j=1;
		k=N-1;
		lij=l[i]-l[j];
		lik=l[i]-l[k];
		A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
		A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
		B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);
		for(i=1;i<N-1;i++)
		{
			j=i+1;
			k=i-1;
			lij=l[i]-l[j];
			lik=l[i]-l[k];
			A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
			A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
			B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);
		}
		i=N-1;
		j=0;
		k=N-2;
		lij=l[i]-l[j];
		lik=l[i]-l[k];
		A.elems[i][0]=(xi[i]-xi[j])*lik-(xi[i]-xi[k])*lij;
		A.elems[i][1]=(yi[i]-yi[j])*lik-(yi[i]-yi[k])*lij;
		B.elems[i][0]=0.5*((pow2(R[i])-pow2(R[j])+pow2(lij))*lik-(pow2(R[i])-pow2(R[k])+pow2(lik))*lij);

		At=A.trans();
		At.print(__debug_out,_T("At=\n"));
		AtA=At*A;
		AtA.print(__debug_out,_T("AtA=\n"));
		PRINT(_T("det(AtA)=%le\n"),AtA.det());
		try
		{
			AtAinv=AtA.inv(EPS);
			AtAinv.print(__debug_out,_T("AtAinv=\n"));
			Apseudo=AtAinv*At;
			Apseudo.print(__debug_out,_T("Apseudo=\n"));
			P=Apseudo*B;

			ex=P.elems[0][0];
			ey=P.elems[1][0];
			e=getLength(ex-x,ey-y);
			PRINT(_T("\nx=%lf, y=%lf error=%le\n"),ex,ey,e);
			_ftprintf(stdout,_T("x=%lf, y=%lf error=%le\n"),ex,ey,e);
			P.destroy();
		}
		catch(...)
		{
			PRINT(_T("Error inversion...\n"));
			printf("ERROR MATRIX INVERSION...\n");
		}
	}
}
void testDLL()
{
	DLLManager dllm(_T("DLLTest.dll"));
}
void testFile()
{
	File file1;
	const char *str="12312345";
	if(file1.open(_T("test1.txt")))
	{
		int i=file1.indexOf((const BYTE*)str,strlen(str));
		_tprintf(_T("%d\n"),i);
	}
}
void testPSum()
{
	const int N=100000;
	double
		x2=0,y2=0,xy=0,
		V[2],V1[2],V2[2],V3[2],P[2][2],
		invP1[2][2],invP2[2][2],invP3[2][2],invP[2][2],
		coef1[2][2],coef2[2][2],coef3[2][2],
		m[2],m1[2]={1,SQRT3/3},m2[2]={-1,SQRT3/3},m3[2]={0,-2*SQRT3/3},
		P1[2][2],P2[2][2],P3[2][2],
		stdevX1=1,stdevY1=2,correl1=0.3,
		stdevX2=2,stdevY2=3,correl2=-0.1,
		stdevX3=1,stdevY3=4,correl3=0.05;
	P1[0][0]=stdevX1*stdevX1;
	P2[0][0]=stdevX2*stdevX2;
	P3[0][0]=stdevX3*stdevX3;
	P1[1][1]=stdevY1*stdevY1;
	P2[1][1]=stdevY2*stdevY2;
	P3[1][1]=stdevY3*stdevY3;
	P1[0][1]=P1[1][0]=correl1*stdevX1*stdevY1;
	P2[0][1]=P2[1][0]=correl2*stdevX2*stdevY2;
	P3[0][1]=P3[1][0]=correl3*stdevX3*stdevY3;
	invMatrix(P1,invP1);
	invMatrix(P2,invP2);
	invMatrix(P3,invP3);
	addMatrix(invP1,invP2,invP);
	addMatrix(invP3,invP);
	invMatrix(invP,P);
	printMatrix(stdout,P,_T("\nP=\n"));
	mulMatrix(P,invP1,coef1);
	mulMatrix(P,invP2,coef2);
	mulMatrix(P,invP3,coef3);
	mulVector(coef1,m1);
	mulVector(coef2,m2);
	mulVector(coef3,m3);
	m[0]=m1[0]+m2[0]+m3[0];
	m[1]=m1[1]+m2[1]+m3[0];
	_tprintf(_T("\nE(x)=%f, E(y)=%f\n"),m[0],m[1]);
	for(int i=0;i<N;i++)
	{
		random_n(V1,V1+1,correl1,stdevX1,stdevY1);
		random_n(V2,V2+1,correl2,stdevX2,stdevY2);
		random_n(V3,V3+1,correl3,stdevX3,stdevY3);
		mulVector(coef1,V1);
		mulVector(coef2,V2);
		mulVector(coef3,V3);
		V[0]=V1[0]+V2[0]+V3[0];
		V[1]=V1[1]+V2[1]+V3[1];
		x2+=V[0]*V[0];
		y2+=V[1]*V[1];
		xy+=V[0]*V[1];
	}
	_tprintf(_T("\nVar(x)=%f, Var(y)=%f, Covar(x,y)=%f,\n"),x2/N,y2/N,xy/N);
}
void testTrilatIterA()
{
	const int N=10,ITERS=100000,SAMPLES=10000,MAX_ITERS=1000;
	const double L=100,stdevL=1.0,stdevX=0.0,stdevY=0.0,correl=0.0,error=0.000001;
	register int i,j,k;
	double
		li[N],varLi[N],Ri[N][2],Pri[N][2][2],realR[2],errRi[2],Rki[2],Rk[2],Rk_1[2],
		dx,dy,d,errX,errY,dR,MerrX,MerrY,VerrX,VerrY,CerrXY,
		stdevLi,stdevXi,stdevYi,correli,errLi,sina,cosa,div,
		P[2][2],invP[2][2],Pi[2][2],Pres[2][2],tmp[2][2],
		invPi[N][2][2],Y[2][2],I_Y[2][2];
	realR[0]=random(L);
	realR[1]=random(L);
	Rk_1[0]=realR[0]+random(-0.01*L,0.01*L);
	Rk_1[1]=realR[1]+random(-0.01*L,0.01*L);
	_tprintf(_T("\nN=%d,realX=%.3f, realY=%.3f, startX=%.3f, startY=%.3f\n\n"),N,realR[0],realR[1],Rk_1[0],Rk_1[1]);
	for(i=0;i<N;i++)
	{
		Ri[i][0]=random(L);
		Ri[i][1]=random(L);
		stdevLi=random(stdevL);
		stdevXi=random(stdevX);
		stdevYi=random(stdevY);
		correli=random(-correl,correl);
		Pri[i][0][0]=stdevXi*stdevXi;
		Pri[i][1][1]=stdevYi*stdevYi;
		Pri[i][0][1]=Pri[i][1][0]=correli*stdevXi*stdevYi;
		varLi[i]=stdevLi*stdevLi;
		// li = ||Ri - realR||
		li[i]=getLength(Ri[i][0]-realR[0],Ri[i][1]-realR[1]);
	}
	
	ClearMemory(P,sizeof(P));
	for(j=0;j<ITERS;j++)
	{
		ClearMemory(invP,sizeof(invP));
		ClearMemory(Rk,sizeof(Rk));
		for(i=0;i<N;i++)
		{
			dx=Rk_1[0]-Ri[i][0];
			dy=Rk_1[1]-Ri[i][1];
			d=getLength(dx,dy);
			// div
			div=li[i]/d;
			// Oi = (Rk_1 - Ri) / ||Rk_1-Ri||
			cosa=dx/d;
			sina=dy/d;
			// Rki = Ri + li * Oi
			Rki[0]=Ri[i][0]+li[i]*cosa;
			Rki[1]=Ri[i][1]+li[i]*sina;
			// Y
			Y[0][0]=			cosa*cosa;
			Y[0][1]=Y[1][0]=	cosa*sina;
			Y[1][1]=			sina*sina;
			// div*(I-Y)
			I_Y[0][0]=			div*(1-Y[0][0]);
			I_Y[0][1]=I_Y[1][0]=-div*Y[0][1];
			I_Y[1][1]=			div*(1-Y[1][1]);
			// (I-Y)*(P+Pri)*(I-Y)'
			addMatrix(P,Pri[i],tmp);
			mulMatrixTrans(I_Y,tmp,Pres);
			// Pi = Pri + VarLi * Oi * Oi'
			Pi[0][0]=			Pri[i][0][0]+varLi[i]*Y[0][0]+Pres[0][0];
			Pi[0][1]=Pi[1][0]=	Pri[i][0][1]+varLi[i]*Y[0][1]+Pres[0][1];
			Pi[1][1]=			Pri[i][1][1]+varLi[i]*Y[1][1]+Pres[1][1];
			invMatrix(Pi,invPi[i]);
			// P^-1 = Sum (Pi^-1)
			addMatrix(invPi[i],invP);
			// Rk = P * Sum (Pi^-1 * Rki)
			Rk[0]+=invPi[i][0][0]*Rki[0]+invPi[i][0][1]*Rki[1];
			Rk[1]+=invPi[i][1][0]*Rki[0]+invPi[i][1][1]*Rki[1];
		}
		invMatrix(invP,P);
		mulVector(P,Rk);
		// error = realR - Rk
		errX=realR[0]-Rk[0];
		errY=realR[1]-Rk[1];
		dR=getLength(Rk_1[0]-Rk[0],Rk_1[1]-Rk[1]);
		if(dR<error)
			break;
		// Rk_1 = Rk
		Rk_1[0]=Rk[0];
		Rk_1[1]=Rk[1];
	}
	_tprintf(_T("iters: %d, err=%10f\n"),j+1,getLength(errX,errY));
	printMatrix(stdout,P,_T("\nest P=\n"));
	if(j>MAX_ITERS)
		return;
	MerrX=MerrY=VerrX=VerrY=CerrXY=0;
	for(k=0;k<SAMPLES;k++)
	{
		for(i=0;i<N;i++)
		{
			random_n(errRi,errRi+1,correli,stdevXi,stdevYi);
			errLi=random_n(stdevL);
			// li = ||Ri - realR||
			li[i]=sqrt(pow2(Ri[i][0]+errRi[0]-realR[0])+pow2(Ri[i][1]+errRi[1]-realR[1]))+errLi;
		}
		for(j=0;j<ITERS;j++)
		{
			ClearMemory(invP,sizeof(invP));
			ClearMemory(Rk,sizeof(Rk));
			for(i=0;i<N;i++)
			{
				dx=Rk_1[0]-Ri[i][0];
				dy=Rk_1[1]-Ri[i][1];
				d=getLength(dx,dy);
				// div
				div=li[i]/d;
				// Oi = (Rk_1 - Ri) / ||Rk_1-Ri||
				cosa=dx/d;
				sina=dy/d;
				// Rki = Ri + li * Oi
				Rki[0]=Ri[i][0]+li[i]*cosa;
				Rki[1]=Ri[i][1]+li[i]*sina;
				// Y
				Y[0][0]=			cosa*cosa;
				Y[0][1]=Y[1][0]=	cosa*sina;
				Y[1][1]=			sina*sina;
				// div*(I-Y)
				I_Y[0][0]=			div*(1-Y[0][0]);
				I_Y[0][1]=I_Y[1][0]=-div*Y[0][1];
				I_Y[1][1]=			div*(1-Y[1][1]);
				// (I-Y)*(P+Pri)*(I-Y)'
				addMatrix(P,Pri[i],tmp);
				mulMatrixTrans(I_Y,tmp,Pres);
				// Pi = Pri + VarLi * Oi * Oi'
				Pi[0][0]=			Pri[i][0][0]+varLi[i]*Y[0][0]+Pres[0][0];
				Pi[0][1]=Pi[1][0]=	Pri[i][0][1]+varLi[i]*Y[0][1]+Pres[0][1];
				Pi[1][1]=			Pri[i][1][1]+varLi[i]*Y[1][1]+Pres[1][1];
				invMatrix(Pi,invPi[i]);
				// P^-1 = Sum (Pi^-1)
				addMatrix(invPi[i],invP);
				// Rk = P * Sum (Pi^-1 * Rki)
				Rk[0]+=invPi[i][0][0]*Rki[0]+invPi[i][0][1]*Rki[1];
				Rk[1]+=invPi[i][1][0]*Rki[0]+invPi[i][1][1]*Rki[1];
			}
			invMatrix(invP,P);
			mulVector(P,Rk);
			// error = realR - Rk
			errX=realR[0]-Rk[0];
			errY=realR[1]-Rk[1];
			dR=getLength(Rk_1[0]-Rk[0],Rk_1[1]-Rk[1]);
			if(dR<error)
				break;
			// Rk_1 = Rk
			Rk_1[0]=Rk[0];
			Rk_1[1]=Rk[1];
		}
		MerrX+=errX;
		MerrY+=errY;
		VerrX+=errX*errX;
		VerrY+=errY*errY;
		CerrXY+=errX*errY;
	}
	MerrX/=SAMPLES;
	MerrY/=SAMPLES;
	P[0][0]=VerrX/SAMPLES-MerrX*MerrX;
	P[1][1]=VerrY/SAMPLES-MerrY*MerrY;
	P[1][0]=P[0][1]=CerrXY/SAMPLES-MerrX*MerrY;
	_tprintf(_T("\n%mx=%10f, my=%10f\n"),MerrX,MerrY);
	printMatrix(stdout,P,_T("real P=\n"));
}
void testTriang()
{
	double	x1=0,y1=0,x2=0.5,y2=0.5*SQRT3,x3=1,y3=0,x=0.3,y=0.5,
			alpha,beta,gama,
			a,b,c,Ra,Rb,Rc,phi1,phi2,phi3,S,Sa,Sb,Sc,
			k1,k2,k3,l12,l22,l32,l1,l2,l3,D,B1,B2,
			cosa,cosb,cosc,sina,sinb,sinc;
	a=getLength(x1-x2,y1-y2);
	b=getLength(x2-x3,y2-y3);
	c=getLength(x3-x1,y3-y1);
	_tprintf(_T("a=%.3f, b=%.3f, c=%.3f, x=%.3f, y=%.3f\n"),a,b,c,x,y);
	l1=getLength(x1-x,y1-y);
	l2=getLength(x2-x,y2-y);
	l3=getLength(x3-x,y3-y);
	_tprintf(_T("l1o=%.3f, l2o=%.3f, l3o=%.3f, x=%.3f, y=%.3f\n"),l1,l2,l3,x,y);
	Sa=triangArea(x1,y1,x2,y2,x,y);
	Sb=triangArea(x2,y2,x3,y3,x,y);
	Sc=triangArea(x3,y3,x1,y1,x,y);
	S=triangArea(x1,y1,x2,y2,x3,y3);
	_tprintf(_T("S=%.3f, Sa=%.3f, Sb=%.3f, Sc=%.3f, sum=%.3f\n"),S,Sa,Sb,Sc,Sa+Sb+Sc);
	cosa=(l1*l1+l2*l2-a*a)/(2*l1*l2);
	cosb=(l2*l2+l3*l3-b*b)/(2*l2*l3);
	cosc=(l3*l3+l1*l1-c*c)/(2*l3*l1);
	sina=2*Sa/(l1*l2);
	sinb=2*Sb/(l2*l3);
	sinc=2*Sc/(l3*l1);
	alpha=atan2(sina,cosa);
	beta=atan2(sinb,cosb);
	gama=atan2(sinc,cosc);
	_tprintf(_T("alpha=%.3f, beta=%.3f, gama=%.3f, sum=%.3f\n"),RAD_TO_DEG*alpha,RAD_TO_DEG*beta,RAD_TO_DEG*gama,RAD_TO_DEG*(alpha+beta+gama));
	Ra=absval(0.5*a/sina);
	Rb=absval(0.5*b/sinb);
	Rc=absval(0.5*c/sinc);
	_tprintf(_T("Ra=%.3f, Rb=%.3f, Rc=%.3f\n"),Ra,Rb,Rc);
	phi1=acos((a*a+c*c-b*b)/(2*a*c));
	phi2=acos((b*b+a*a-c*c)/(2*b*a));
	phi3=acos((c*c+b*b-a*a)/(2*c*b));
	_tprintf(_T("phi1=%.3f, phi2=%.3f, phi3=%.3f, sum=%.3f\n"),RAD_TO_DEG*phi1,RAD_TO_DEG*phi2,RAD_TO_DEG*phi3,RAD_TO_DEG*(phi1+phi2+phi3));
	k1=cos(beta-phi1);
	k2=cos(gama-phi2);
	k3=cos(alpha-phi3);
	_tprintf(_T("k1=%.3f, k2=%.3f, k3=%.3f\n"),k1,k2,k3);
	l12=4*(Ra*Ra*Rc*Rc*(1-k1*k1))/(Ra*Ra+Rc*Rc+2*Ra*Rc*k1);
	l22=4*(Rb*Rb*Ra*Ra*(1-k2*k2))/(Rb*Rb+Ra*Ra+2*Rb*Ra*k2);
	l32=4*(Rc*Rc*Rb*Rb*(1-k3*k3))/(Rc*Rc+Rb*Rb+2*Rc*Rb*k3);
	l1=sqrt(l12);
	l2=sqrt(l22);
	l3=sqrt(l32);
	D=(x1-x2)*(y1-y3)-(x1-x3)*(y1-y2);
	_tprintf(_T("l1=%.3f, l2=%.3f, l3=%.3f, 0.5*D=%.3f\n"),l1,l2,l3,0.5*D);
	B1=(x1*x1-x2*x2+y1*y1-y2*y2-l12+l22)/(2*D);
	B2=(x1*x1-x3*x3+y1*y1-y3*y3-l12+l32)/(2*D);
	x=(y1-y3)*B1+(y2-y1)*B2;
	y=(x3-x1)*B1+(x1-x2)*B2;
	_tprintf(_T("B1=%.3f, B2=%.3f, x=%.3f, y=%.3f\n"),B1,B2,x,y);
}

#define N 10000000
void testSqrt()
{
	PrecisionTimer timer;
	int i;
	float s1,s2,n = (float)random(0.01,100);
	double t,t1,t2;

	s1=INVSQRTF(n);
	s2=FastInvSqrt(n);
	_tprintf(_T("n=%f, 1/sqrt=%f, fast=%f\n"), n, s1, s2);

	Vector<float> r,v;
	if(!r.setSize(N)||!v.setSize(N))
	{
		_tprintf(_T("Alloc error!\n"));
		return;
	}
	timer.restart();
	for(i=0;i<N;i++)
		r[i]=(float)random(0.01,100);
	t=timer.restart();
	

	_tprintf(_T("rand time = %f\n"), t);

	timer.restart();
	for(i=0;i<N;i++)
		v[i]=INVSQRTF(r[i]);
	t1=timer.restart();

	_tprintf(_T("1/sqrt time = %f\n"), t1);

	timer.restart();
	for(i=0;i<N;i++)
		v[i]=FastInvSqrt(r[i]);
	t2=timer.restart();

	_tprintf(_T("fast time = %f (about %.0f times faster)\n"), t2, t1/t2);
}
typedef int (*FUNC)(int);
int f1(int x){return x;}
int f2(int x){return x;}
int f3(int x){return x;}
int f4(int x){return x;}
int f5(int x){return x;}
void testSwitch()
{
	int i,*rn,n;
	static FUNC f[]={&f1,&f2,&f3,&f4,&f5};
	const int m=sizeof(f)/sizeof(f[0]);
	PrecisionTimer timer;
	double t,t1,t2,r=-1,h;
	Vector<int> vN;

	rn=vN.setSize(N);
	timer.restart();
	for(i=0;i<N;i++)
	{
		n=__int(random(m));
		assert(n!=m);
		rn[i]=n;
	}
	t=timer.restart();
	_tprintf(_T("rand time = %f\n"), t);

	h=0;
	timer.restart();
	for(i=0;i<N;i++)
	{
		n=rn[i];
		switch(n)
		{
		case 0:
			r=f1(n);
			break;
		case 1:
			r=f2(n);
			break;
		case 2:
			r=f3(n);
			break;
		case 3:
			r=f4(n);
			break;
		case 4:
			r=f5(n);
			break;
		}
		h=0.5*(h+r);
	}
	t1=timer.restart();
	_tprintf(_T("switch time = %f, h = %f\n"), t1, h);

	h=0;
	timer.restart();
	for(i=0;i<N;i++)
	{
		n=rn[i];
		r=f[n](n);
		h=0.5*(h+r);
	}
	t2=timer.restart();
	_tprintf(_T("table time = %f, h = %f\n"), t2, h);
}
void testSleep()
{
	PrecisionTimer timer;
	int sleep=10;

	timer.restart();
	Sleep(sleep);
	_tprintf(_T("Sleep error: %f ms\n"), sleep-1000*timer.time());
}
void testRandSeq()
{
	DWORD n = 10000, i, x = rand() % n,mini,maxi;
	Vector<double>buff;
	double c,m,v,den,ac,maxc,minc;

	if(!buff.setSize(n))
	{
		_tprintf(_T("Alloc error!\n"));
		return;
	}
	
	_tprintf(_T("n=%d\n"),n);
	for(i=0;i<n;i++)
	{
		x = PseudoRandomSequence(x, n);
		buff[i]=x;
		//_tprintf(_T("%d "), x);
	}
	m=mean(buff,n);
	v=var(buff,n,m);
	_tprintf(_T("\nmean=%f, var=%f\n"), m, v);
	den=INV(v);

	ac=autocorrel(buff,n,1,0);
	c=(ac-m*m)*den;
	mini=maxi=1;
	maxc=minc=c;

	for(i=2;i<n;i++)
	{
		ac=autocorrel(buff,n,i,0);
		c=(ac-m*m)*den;
		if(abs(maxc)<abs(c))
		{
			maxc=c;
			maxi=i;
		}
		else if(abs(minc)>abs(c))
		{
			minc=c;
			mini=i;
		}
		//_tprintf(_T("%.2f "), (ac-m*m)*den);
	}
	_tprintf(_T("\nminc=% f, mini=%d\n"), minc, mini);
	_tprintf(_T("maxc=% f, maxi=%d\n"), maxc, maxi);
}
void testDataConvert()
{
	int n = 19945;
	char s[128] = "";
	numToStr(n,s,10,16,TRUE);
	n=strToNum(s,10,16);
	_tprintf(_T("n = %s\n"),s);
	_tprintf(_T("s = %d\n"),n);
}
void testDataShift()
{
	int t[]={1,2,3,4,5,6};
	Vector<int> v;
	v.bind(t,sizeof(t)/sizeof(int));
	v.print(_T("%d, "));
	_tprintf(_T("\n"));
	v.shift(-3);
	v[3]=7;
	v[4]=8;
	v[5]=9;
	v.print(_T("%d, "));
	_tprintf(_T("\n"));
	v.shift(3);
	v[0]=1;
	v[1]=2;
	v[2]=3;
	v.print(_T("%d, "));
	_tprintf(_T("\n"));
}
#define SIZE 10
void testIntrinsic()
{
	_tprintf(_T("fail1\n"));
	Vector<float4> B(MEM_TYPE_ALLOC,16);
	Table<float4> T(MEM_TYPE_ALLOC,16);
	float4 bf2[SIZE];
	float4 *bf3;
	float4 *bf4;

	NEW_ARRAY(bf3,float4,SIZE);
	NEW_ALIGNED(bf4,float4,SIZE*sizeof(float4),16);
	
	float4 *bf=B.setSize(SIZE,TRUE);
	float4 **t=T.setSize(SIZE,SIZE,TRUE);
	
	Vector<float4> BC(B);
	Table<float4> TC(T);
	
	float4 a = {1,2,3,4};
	float4 b = {1,2,3,4};
	float4 c = {1,2,3,4};
	float4 d = {1,2,3,4};
	float4 r = sqrt(a * b - rcp(c))/d;
	bf2[0]=bf4[0]=t[0][0]=r;
	//bf3[0]=r;
	if(r == a)
	{
		_tprintf(_T("fail1\n"));
	}
	else if(a != b)
	{
		_tprintf(_T("fail2\n"));
	}
	_tprintf(_T("{%f,%f,%f,%f}\n"),float4Unpack(r));
	DELETE_ARRAY(bf3);
	DELETE_ALIGNED(bf4);
}
void testList()
{
	List<_TCHAR> list;
	list.add('A');
	list.add('D');
	list.add('Z');
	list.add('B');
	list.add('E');
	Iterator<_TCHAR> iter(&list);
	while(iter.next()) _tprintf(_T("%c"),*iter.value); _tprintf(_T("\n"));
	list.remove(2);
	while(iter.next()) _tprintf(_T("%c"),*iter.value); _tprintf(_T("\n"));
	list.replace(list.first->next,list.last->prev);
	while(iter.next()) _tprintf(_T("%c"),*iter.value); _tprintf(_T("\n"));
	list.add('C',2);
	while(iter.next()) _tprintf(_T("%c"),*iter.value); _tprintf(_T("\n"));
}
#define Z 1000
void testZip()
{
	int *b,a[Z];
	for(int i=0;i<Z;i++)
		a[i]=i;
	Compressor c,d;
	c.compress((BYTE*)a,sizeof(a));
	PRINT(_T("RATIO %.2f%%\n"),100*(1-c.size*INV(sizeof(a))))
	d.decompress(c);
	b=(int*)d.elems;
	for(int i=0;i<Z;i++)
		if(a[i]!=b[i])
			PRINT(_T("ERROR %d\n"),i);
}




void testSha1()
{
	Sha1 hash1 = "tova e test";
	Sha1 hash2 = hash1;
	Sha1 hash3("tova e test!");
	hash1.print();
	printf("\n8fe821f551609c3769778f0fe8a893bcde52f7de\n");
	printf("hash1==hash2 ? %d\n", hash1 == hash2);
	printf("hash1==hash3 ? %d\n", hash1 == hash3);
}


template <class T>
class __Branch
{
public:
	List<__Branch> branches;
	char key;
	T *value;
	__Branch():key(0),value(NULL)
	{}
	~__Branch()
	{
		if(value)
			DELETE_OBJECT(value);
	}
	BOOL set(const T& value)
	{
		if(!create())
			return FALSE;
		*this->value = value;
		return TRUE;
		
	}
	BOOL get(T& value)
	{
		if(!this->value)
			return FALSE;
		value = *this->value;
		return TRUE;
	}
	BOOL create()
	{
		return this->value || NEW_OBJECT(this->value,T);
	}
	__Branch *get(char key)
	{
		for(List<__Branch>::Object obj = branches.first; obj; obj=obj->next)
			if(obj->value.key == key)
				return &obj->value;
		return NULL;
	}
	__Branch *add(char key)
	{
		__Branch *branch = branches.add();
		if(branch)
			branch->key = key;
		return branch;
	}
};
template <class T, unsigned int size>
class __Map
{
	typedef __Branch<T> Branch;
protected:
	Branch trunc;
	Branch *__branch(const void *key, bool create = false)
	{
		if(!key)
			return NULL;
		unsigned int i=0;
		char *k = (char *)key;
		Branch *branch = &trunc;
		for(;;)
		{
			Branch *next_branch = branch->get(*k);
			if(!next_branch)
			{
				if(!create)
					return NULL;
				next_branch = branch->add(*k);
				if(!next_branch)
					return NULL;
			}
			branch = next_branch;
			k++;
			if(size)
			{
				i++;
				if(i==size)
					break;
			}
			else if(!(*k))
			{
				break;
			}
		}
		if(create&&!branch->create())
			return NULL;
		return branch;
	}
	BOOL __set(const void *key, const T &value)
	{
		Branch *branch = __branch(key, true);
		if(!branch)
			return FALSE;
		return branch->set(value);
	}
	BOOL __get(const void *key, T &value)
	{
		Branch *branch = __branch(key);
		if(!branch)
			return FALSE;
		return branch->get(value);
	}
public:
	void clear()
	{
		trunc.branches.destroy();
	}
	__Map()
	{}
	~__Map()
	{
		clear();
	}
};

template<class K, class T>
class Map: public __Map<T, sizeof(K)>
{
public:
	BOOL set(const K& key, const T& value)
	{
		return __set(&key, value);
	}
	BOOL get(const K& key, T& value)
	{
		return __get(&key, value);
	}
	T& operator[](const K& key)
	{
		return *__branch(&key, true)->value;
	}
};

template<class T>
class Map<CSTR, T>: public __Map<T, 4>
{
public:
	BOOL set(CSTR key, const T& value)
	{
		DWORD hash = toHash(key);
		return __get(&hash, value);
	}
	BOOL get(CSTR key, T& value)
	{
		DWORD hash = toHash(key);
		return __get(&hash, value);
	}
	T& operator[](CSTR key)
	{
		DWORD hash = toHash(key);
		return *__branch(&hash, true)->value;
	}
};

void testMap()
{
	Map<int,int> map1;
	map1[2] = 1;
	PRINT("map[2]=%d\n", map1[2]);
	map1[10] = 3;
	PRINT("map[10]=%d\n", map1[10]);

	Map<CSTR,String> map2;
	map2["two as cc scasc"] = "12";
	PRINT("map[two as cc scasc]=%s\n", (CSTR)map2["two as cc scasc"]);
	CSTR str = "two as cc scasc";
	PRINT("map[two as cc scasc]=%s\n", (CSTR)map2[str]);
	map2["ten"] = "31";
	PRINT("map[ten]=%s\n", (CSTR)map2["ten"]);
}

#define MN 33
#define C 1000
void testMatrixStatic()
{
	/*
	M<3,3> m;
	m.rand();
	M<3,3> m1 = m;
	printf("m1 == m = %d\n", m1 == m);
	printf("m1 == m' = %d\n", m1 == m.trans());
	printf("det = %f\n", m1.det());
	m1.print();
	(m1 + m - 2*m).print();
	(m1 * m1.inv()).print();
*/
	double t;
	PrecisionTimer timer;
	M<MN> mx2, mx1;
	double t1 = 1000*timer.restart();
	
	mx2.rand();
	mx1.rand();

	timer.restart();
	Matrix m1(MN,MN), m2(MN,MN);
	double t2 = 1000*timer.restart();
	double dd = 1;
	for(int i=0;i<MN;i++)
	{
		dd *= mx1(i, i);
		for(int j=0;j<MN;j++)
		{
			m1.elems[i][j] = mx1(i, j);
			m2.elems[i][j] = mx2(i, j);
		}
	}
	PRINT("det dyn %f, det stat %f\n", m1.det(), mx1.det());
	int i;
	timer.restart();t=0;for(i=0;i<C;i++)
		t += mx1.det();
	double n3 = t/(C*MN*MN), t3 = (1000.0/C)*timer.restart();

	timer.restart();t=0;for(i=0;i<C;i++)
		t += m1.det();
	double n4 = t/(C*MN*MN), t4 = (1000.0/C)*timer.restart();

	timer.restart();t=0;for(i=0;i<C;i++)
		t += (mx1.trans() * mx2)(0, 0);
	double n5 = t/C, t5 = (1000.0/C)*timer.restart();

	timer.restart();t=0;for(i=0;i<C;i++)
		t += (m1.trans() * m2)[0][0];
	double n6 = t/C, t6 = (1000.0/C)*timer.restart();

	timer.restart();t=0;for(i=0;i<C;i++)
		t += (mx1.inv() * mx1).trace();
	double n7 = t/(C*MN), t7 = (1000.0/C)*timer.restart();
	
	timer.restart();t=0;for(i=0;i<C;i++)
		t += (m1.inv() * m1).trace();
	double n8 = t/(C*MN), t8 = (1000.0/C)*timer.restart();
	
	//*
	timer.restart();
	t = (mx1.inv() * mx1).trace();
	PRINT("static   d1 = %f (%.3f)\n", t/MN, 1000*timer.restart());

	timer.restart();
	t = (m1.inv() * m1).trace();
	PRINT("dynamic  d1 = %f (%.3f)\n", t/MN, 1000*timer.restart());

	timer.restart();
	t = (mx1.pseudoInv() * mx1).trace();
	PRINT("static   d2 = %f (%.3f)\n", t/MN, 1000*timer.restart());

	timer.restart();
	t = ((m1.trans() * m1).inv() * m1.trans() * m1).trace();
	PRINT("dynamic  d2 = %f (%.3f)\n", t/MN, 1000*timer.restart());

	timer.restart();
	MatrixInvertor mi;
	if(mi.bind(&m1) && mi.inv())
	{
		t = (mi.mInvM * m1).trace();
		PRINT("dynamic* d2 = %f (%.3f)\n", t/MN, 1000*timer.restart());
	}
	/**/

	PRINT("\nstatic creation timer = %.3f\n", t1);
	PRINT("dynamic creation timer = %.3f\n", t2);
	PRINT("static det = %le, timer = %.3f\n", n3, t3);
	PRINT("dynamic det = %le, timer = %.3f\n", n4, t4);
	PRINT("static mul = %le, timer = %.3f\n", n5, t5);
	PRINT("dynamic mul = %le, timer = %.3f\n", n6, t6);
	PRINT("static inv = %lf, timer = %.3f\n", n7, t7);
	PRINT("dynamic inv = %lf, timer = %.3f\n", n8, t8);
}

int Start(int argn, char ** argv)
{
	INITRAND;

	_tprintf(_T("Changing process priority from 0x%X\n"),GetPriorityClass(GetCurrentProcess()));
	if(!SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS))
		_tprintf(_T("Failed to change priority: %s\n"), getLastErrorStr());
	_tprintf(_T("Current priority class is 0x%X\n"),GetPriorityClass(GetCurrentProcess()));

	TEMP_DEBUG_OUT(_T("test.txt"));
	PrecisionTimer timer;
	try
	{
		testMatrixStatic();
	}
	catch(...)
	{
		_tprintf(_T("Exception!\n"));
		return 1;
	}
	_tprintf(_T("\nDone in %.3f ms\n"), 1000*timer.time());
	PRINTMEM(TRUE);
	return 0;
}

}; // namespace Lio

#define F abc
#define B def
#define FB(arg) #arg
#define FB1(arg) FB(arg)
  


int main(int argn, char ** argv)
{
	Lio::Start(argn, argv);

	return 0;
}